
## NOTES TO EDITORS
# todo: Implement New Gamecontroller
# TODO: Fix locategoal
# TODO: Fix doorlopen when penalized
# TODO: Cleaning visioninterface

## FILE DESCRIPTION
# File: soul.py
# Description: The nao's footballplaying soul.
# * states call phases
# * phases call moves
# * Transitions between states through global int state
# * Transitions between phases through global string phase
# state : e.g. 1
# phase : e.g. 'BallFound'

from naoqi import ALProxy
import gameStateController
import threading
import time
import math    
import Queue
import motionInterface as mot
import visionInterface as v

# start the ballfinding thread
vis = v.Vision('VISION')
vis.start()

# audio settings
speak = ALProxy('ALTextToSpeech', '127.0.0.1', 9559)
audio = ALProxy('ALAudioDevice', '127.0.0.1', 9559)
audio.setOutputVolume(90)

# used for keeper, TODO implement as dictionary? (not sure if faster but looks neat)
ball_loc = dict()

# Initialization of archtitecture
state = 0
phase = 'BallNotFound'

# PROXIES
leds = ALProxy('ALLeds', '127.0.0.1', 9559)
video = ALProxy('ALVideoDevice', '127.0.0.1', 9559)
video.setParam(18,1)

# Keep track of first calls, to see if it's the first run
firstCall = {'Initial' : True, 'Ready' : True, 'Set': True, 'Playing': True, 'Penalized': True, 'BallFoundKeep' : True}

# Gamecontroller
sc = gameStateController.stateController('stateController')
gc = sc.gc          #Do not use this for getting the game state, use sc instead

# robots own variables, taken from naoinfo.txt line 1 and 2
robot = sc.getRobotNumber()
playerType = 0
if (robot == 0):
    playerType = 1
else:
    playerType = 0
    
# Try to connect to gameController, initialize (and change) variables used for kickoff and other phases
teamColor = None
kickOff = None
penalty = None
if gc.controlledRefresh():
    teamColor = gc.getTeamColor('we')
    kickOff = gc.getKickoff()
else:
    if sc.getSetup():
        (teamColor, penalty) = sc.setup(0, 0)
 
    
## MOTIONS FUNCTIONS
# normalPose()
# post.walkto()
# v.scanCircle()
# standup()
# rKickAngled()
# lKickAngled()
# hakje()
# FootRight()
# FootLeft()
# DiveRight()
# DiveLeft()
# stance()

## STATES (Gamestates)
# Initial()
# Ready()
# Set()
# Playing()
# Penalized()
# Finished()


# Initial state: do nothing, stand ready for match
# LEDS: All leds are off
# Motions: NormalPose
def Initial():
    global phase                    # to change, call global, else not possible
    global ball_loc
    global firstCall
    
    if firstCall['Initial']:
        print 'In initial state'
        vis.stopBall()                  # do not find the ball when in Initial
        print 'TeamColor' , teamColor   # print the teamcolor as a check

        # empty variables
        ball_loc = dict()
        
        
        leds.off('AllLeds')         # turn off all leds
        mot.stiff()                 # stiffness on
        mot.normalPose()            # normalPose
        
        firstCall['Initial']   = False   
        firstCall['Ready']     = True
        firstCall['Set']       = True
        firstCall['Playing']   = True
        firstCall['Penalized'] = True
        
    # footleds display teamcolor , not necessary
    #if (teamColor == 0):
    #    leds.fadeRGB('RightFootLeds', 0x000000ff, 0)
    #    leds.fadeRGB('LeftFootLeds', 0x000000ff, 0)
    #else:
    #    leds.fadeRGB('RightFootLeds', 0x00ff0000, 0)
    #    leds.fadeRGB('LeftFootLeds', 0x00ff0000, 0)
    

# Ready state: possible positioning
# LEDS:  Chest Blue
#        Feet  Teamcolor
def Ready():
    global phase
    global firstCall
    
    # reinitialize phases, stand ready for match
    if firstCall['Ready']:
        print 'In ready state'
        mot.killWalk()
        leds.fadeRGB('ChestLeds',0x000000ff, 0) # set chestleds

        # if a player
        if playerType == 0:
            phase = 'BallNotFound'
            mot.move('normalPose')
        # if a keeper
        else:
            phase = 'BallNotFoundKeep'
            mot.move('keepNormalPose')
            
        firstCall['Initial'] = True   
        firstCall['Ready']   = False
        firstCall['Set']     = True
        firstCall['Playing'] = True
        firstCall['Penalized'] = True
        
# Set state: start searching for ball
# LEDS:  Chest Yellow
def Set():
    global teamColor
    global kickOff
    global phase
    global firstCall
    
    # if the first iteration
    if firstCall['Set']:
        print 'In set state'
        vis.goBall()                                # start ballscan
        
        if gc.controlledRefresh():                  # update variables teamcolor, kickoff
            teamColor = gc.getTeamColor('we')
            kickOff = gc.getKickoff()
        
        print 'TeamColor' , teamColor               # print teamcolor as a check
        mot.stiff()    
        
        if playerType == 0:
            mot.normalPose()
        else:
            mot.keepNormalPose()
        audio.setOutputVolume(0)                    # set volume to zero    
        leds.fadeRGB('ChestLeds',0x00ffff00, 0)     # set chestledcolor to green
        
        firstCall['Initial']   = True   
        firstCall['Ready']     = True
        firstCall['Set']       = False
        firstCall['Playing']   = True
        firstCall['Penalized'] = True

    # head movements allowed in the set state
    #  FIND A BALL  #
    if phase != 'BallFound' and phase != 'BallFoundKeep':
        if v.scanCircle(0.2, vis):
            if playerType == 0:
                phase = 'BallFound'
            else:
                phase = 'BallFoundKeep'
        else:
            if playerType == 0:
                phase = 'BallNotFound'
            else:
                phase = 'BallNotFoundKeep'
    # keep finding the ball, it might move (or the nao moves)
    if not(vis.findBall()):
            if playerType == 0:
                phase = 'BallNotFound'
            else:
                phase = 'BallNotFoundKeep'

        
# Playing state: play game according to phase transitions
# LEDS:  Chest Green
def Playing():
    global phase
    global firstCall
    
    if firstCall['Playing']:
        print 'In playing state'
        leds.fadeRGB('ChestLeds', 0x0000ff00, 0)
        firstCall['Initial']   = True   
        firstCall['Ready']     = True
        firstCall['Set']       = True
        firstCall['Playing']   = False
        firstCall['Penalized'] = True
        
    # if nao has fallen down, stand up
    if mot.standUp():
        if playerType == 0:
            # and change phase to ballnotfound
            phase = 'BallNotFound'
        else:
            phase = 'BallNotFoundKeep'
            
    # Execute the phase as specified by phase variable
    phases.get(phase)()

# Penalized state
# LEDS:  Chest Red
def Penalized():
    global phase
    global firstCall
    
    if firstCall['Penalized']:
        print 'In penalized state'
        vis.stopBall()          # stop looking for the ball
        mot.killWalk()          # stop walking
        mot.stiff()             # stiffness on (just in case?)
        mot.keepNormalPose()
        leds.fadeRGB('ChestLeds',0x00ff0000, 0) # leds on chest: red
        if playerType == 0:     # if a player, goto unpenalized phase
            phase = 'Unpenalized'
        else:                   # if a keeper, go to ballnotfoundkeep if penalty shootout
            if penalty:
                phase = 'BallNotFoundKeep'
            else:               # else, become a player
                phase = 'Unpenalized'

        firstCall['Initial']   = True   
        firstCall['Ready']     = True
        firstCall['Set']       = True
        firstCall['Playing']   = True
        firstCall['Penalized'] = False
                
def Finished():
    vis.stopBall()                  # stop looking for the ball
    leds.off('AllLeds')             # turn off leds
    mot.killWalk()                  # stop walking
    mot.stance()                    # sit down
    mot.setHead((0,0))              # set head straight
    time.sleep(1)                   # wait until seated etc
    mot.killKnees()                 # stiffness off to prevent heating
    #sc.close()
    audio.setOutputVolume(85)       # volume on
    
    
## PHASES

# KEEPER
# BallFoundKeep()
# BallNotFoundKeep()
# BallApproaching()
# InGoalArea()

def BallFoundKeep():
    global phase
    global ball_loc
    global firstCall
    
    if firstCall['BallFoundKeep']:
        mot.move('stance')
        firstCall['BallFoundKeep'] = False
        
    maxlength = 6
    halfmaxlength = (maxlength/2.0)

    #  FIND A BALL  #
    ball = vis.findBall()        
    if ball:    
        #if ball[0] < 0.3 and (ball[1] < 0.5 or ball[1] > -0.5):
        #    phase = 'InGoalArea'
        #    mot.move('normalPose')
        #    vis.findBall()
        #    return True
        
        if len(ball_loc) == 0:
            ball_loc[ 0 ] = ball
            
        elif len(ball_loc) < maxlength:
            # add to dict, key being a number from maxlength to 0. Last position is position 8 (key 7)
            # but only if ball_loc is already filled
            if ball != ball_loc[ len(ball_loc) - 1 ]:
                ball_loc[ len(ball_loc) ] = ball
        
        elif ball != ball_loc[ maxlength - 1 ]:
            xold = 0
            yold = 0
            xnew = 0
            ynew = 0

            # shift elements sidewards, newest ballloc becomes nr <maxlength>, oldest is thrown away
            for number in ball_loc:
                (x,y) = ball_loc[number]
                if number != 0:
                    ball_loc[number-1] = (x,y)
                            
                # add to xold/new and yold/new variables, number 0 is oldest, number <maxlength> most recent ballloc
                if number == 0:
                    pass
                elif 0 < number < halfmaxlength + 1:
                    xold += x / halfmaxlength
                    yold += y / halfmaxlength
                else:
                    xnew += x / halfmaxlength
                    ynew += y / halfmaxlength
            
            xnew += ball[0] / halfmaxlength
            ynew += ball[1] / halfmaxlength
            ball_loc[maxlength-1] = ball
                
            # calc diff in distance
            distold = math.sqrt(xold**2 + yold**2)
            distnew = math.sqrt(xnew**2 + ynew**2)
            speed = distold - distnew
            
            print 'Ball moving from ', xold, yold, 'to', xnew, ynew, '(mean). Speed', speed
            
            # calculate direction if speed is high enough
            if speed > 0.225:
                #mot.stiffKnees()
                
                # This is all triangular magic using similarity between two triangles formed by balllocations and Nao.
                # Keep in mind that y-axis is inverted and that x is forward, y is sideways!
                #
                #                                              x 
                #      (yold,xold)                             |
                #            .                                 |    
                #            | \                               |
                #          B |   \                             |
                #            |     \                           |
                #            |_______.  (ynew,xnew )           |
                #                A   | \                       |
                #                    |   \                     |
                #                  C |     \                   |
                #                    |       \                 |
                #     y _____________|_________._______________.___________ -y
                #                            (dir,0)           Nao = (0,0)
                #                               
                #                    [--- D ---]
                
                # Mathematical proof:
                # A = yold - ynew
                # B = xold - xnew
                # C = xnew
                # D = ynew - dir
                
                # Similar triangles -> C / B = D / A 
                #                      D     = A*C/B
                #                      D     = ynew - dir
                #                      dir   = A*C/B - ynew
                
                dir = (yold - ynew )* xnew / (xold - xnew) - ynew
            
                ball_loc = dict()
                vis.clearCache()
                
                ball_loc['Direction'] = dir
                phase = 'BallApproaching'
                firstCall['BallFoundKeep'] = True
                print 'Direction', dir
        
    else:
        #mot.stiffKnees()
        phase = 'BallNotFoundKeep'
        firstCall['BallFoundKeep'] = True
        
# Keeper lost track of ball
def BallNotFoundKeep():
    global phase
    global ball_loc
        
    # if a ball is found
    if v.scanCircle(0.2, vis):
        # reinitialize
        ball_loc = dict()
        phase = 'BallFoundKeep'
    else:
        # stand up
        mot.keepNormalPose()

# Ball approaches keeper and goal area at considerable speed
def BallApproaching():
    global phase
    global ball_loc

    dir = ball_loc['Direction']
    
    if dir >= 0.25:
        print 'Dive right'
        mot.diveRight()
    elif dir <= -0.25:
        print 'Dive left'
        mot.diveLeft()
    elif dir < 0.25 and dir >= 0:
        print 'Step right'
        mot.footRight()
    elif dir > -0.25 and dir < 0:
        print 'Step left'
        mot.footLeft()
    phase = 'BallNotFoundKeep'
    
def InGoalArea():
    global phase
    global mean
    mot.move('normalPose')
    ball = vis.findBall()
    if ball:
        o_x = ball[0] - 0.15
        o_y = ball[1]
        theta = math.atan2(o_y, o_x)
    
        x = 0
        y = 0
        # walk towards the ball (this means it may not be moving. at all)
        while ball:
            ball = vis.findBall()
            if ball:
                (x,y) = ball
                print x,y
                if x < 0.19 and -0.04 > y > -0.08:
                    # BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE RESULTS 
                    mot.killWalk()
                    ball = vis.findBall()
                    # correct one last time
                    if ball:
                        (x,y) = ball
                        mot.move('walkTo', (0, y + 0.04,0))
                        time.sleep(0.5)
                        print 'Kick'
                        mot.move('rKickAngled', 0)
                        mot.move('normalPose')
                        break

                if x > 0.3 and (y > 0.2 or y < 0.2):
                    x = x * 2
                    y = y * 2
                if x > 1:
                    x = 1
                if y > 1:
                    y = 0.96
                elif y < -1:
                    y = -0.96
                if x >= -1:
                    mot.move('SWTV', (x-0.15,y+0.05,0, 1) )
            else:
                print 'ball lost, returning'
                
        mot.move('walkTo', ((x-0.15)-o_x, (y-0.04)-o_y, -theta))
    mot.keepNormalPose()
    phase = 'BallNotFoundKeep'

'''
def UnpenalizedKeep():
    global phase
    global ball_loc
    while  not( ball_loc.empty() ):
        ball_loc.get()
    ball_loc.put( (10, 10, 0) )
    phase = 'ReturnToGoal'

def ReturnToGoal():
    if mot.isWalking():
        return True
    
    # stored location of the goal (how to get there)
    (realX,realY,angle) = ball_loc.get()
    tempGoalPostBlue = None
    tempGoalPostYellow = None
    goal = None
    # color 0 is blue
    
    if realX > 0.75 or realY > 0.2 or realY < -0.2:
        for angle in [0, -0.5, 0.5, -1, 1, -1.5, 1.5, -2, 2]:
            mot.move('setHeadBlock', (angle, -0.55, 0.6))
            #print 'Looking for goal in angle ', angle
            goal = vis.findGoal(tempGoalPostBlue, tempGoalPostYellow)
            if goal:
                if goal[0] == 'Blue' and goal[1] == 'Post' and not tempGoalPostBlue:
                    tempGoalPostBlue = goal[2]
                elif goal[0] == 'Yellow' and goal[1] == 'Post' and not tempGoalPostYellow:
                    tempGoalPostYellow = goal[2]
                elif goal[1] == "Goal":
                    break
        if goal:
            #if (goal[0] == 'Blue' and teamColor == 1) or (goal[0] == 'Yellow' and teamColor == 0):
            #    turn around
            if type(goal[2]) == str or type(goal[2]) == dict:
                print 'saw', goal[2]
                mot.move('walkTo', (-0.2,0,0)) 
            else:
                leds.fadeRGB('LeftFaceLeds',0x0000ff00, 0) #goal, led turns green
                headangle = angle
                goal = goal[2] # info is stored in a tuple, 3d position
                realX = goal[0]
                realY = goal[1]
                angle = goal[2]
                
                # don't walk all the way
                if realX < 1.5:
                    x = realX / 2.0
                else:
                    x = realX
                if (0 < realY < 1.5) or (-1.5 < realY < 0):
                    y = realY / 2.0
                else:
                    y = realY
                mot.move('setHead', ( angle / 2.0, -0.55))
                mot.move('postWalkTo', (x / 2.0, y / 2.0, angle / 2.0))
                print goal
                

        else:
            mot.move('walkTo', (0, 0, 1))
            
    if realX < 0.75 and  -0.2 < realY < 0.2:
        if angle > 0:
            mot.move('walkTo', (0,realY, 3.1415 - goal[2]))
        else:
            mot.move('walkTo', (0,realY, 3.1415 + goal[2]))
        mot.move('walkTo',(0.1, 0, 0))
        mot.keepNormalPose()
        phase = "BallNotFoundKeep"
    ball_loc.put( (realX,realY,angle) )
'''
    
            
# PLAYER
# BallFound()
# BallNotFound()
# Kick()
# Unpenalized()
# ReturnField()

def BallFound():
    leds.fadeRGB('RightFaceLeds', 0x0000ff00, 0)
    global phase
    
    # FIND A BALL #
    ball = vis.findBall()
    if ball:
        (x,y) = ball
        print x,y
        if x < 0.17 and -0.02 < y < 0.02:
            print 'Kick'
            # BLOCKING CALL: FIND BALL WHILE STANDING STILL FOR ACCURATE CORRECTION
            mot.killWalk()
            phase = 'Kick'
            time.sleep(3)
            return True
        
        theta = math.atan(y/x)
        
        
        x = 3 * (x-0.17)
        
        if  -0.1 < y < 0.1 :
            y = 0.5 * y
        else:
            y = 2.0 * y
        
        if x > 1:
            x = 1
        if y > 1:
            y = 1
        if y < -1:
            y = -1
        if x >= -1:
            mot.move('SWTV', (x , y, theta/2.5 , 1.0) )
    else:
        print 'No Ball'
        mot.killWalk()
        phase = 'BallNotFound'

def BallNotFound():
    global ball_loc
    
    leds.fadeRGB('RightFaceLeds',0x00ff0000, 0) # no ball, led turns red
    global phase
    # try to find a ball
    if v.scanCircle(0.2, vis):
        phase = 'BallFound'
    # if no ball is found
    else:
        # circle slowly
        mot.move('postWalkTo', (0, 0, 1.3))
        
def Kick():
    global phase
    vis.goGoal()   # start goalfinding
    leds.fadeRGB('RightFaceLeds',0x00000000, 0) # led turns black
    leds.fadeRGB('LeftFaceLeds', 0x00ff0000, 0) #no goal, led turns red
    
    # try different positions of the head
    goal = v.scanCircleGoal(3, vis)
    vis.goBall()
    
    # Case 0 : Ball stolen.
    # Check if the ball is still there
    mot.move('setHead', (0, 0.5))
    # wait until ball is found or 1 second has passed
    now = time.time()
    while time.time() - now < 1 and not(vis.findBall()):
        pass
    # needs more escape clauses if no ball is found during this time
    ball = vis.findBall()
    if not ball:
        print 'Ball gone'
        phase = 'BallNotFound'
        leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
        return False
    elif ball[0] > 0.25 or ball[1] > 0.1 or ball[1] < -0.1:
        print 'Ball too far'
        phase = 'BallFound'
        leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
        return False    
    elif ball and not goal:
        mot.move('walkTo', (0,0.04 + ball[1],0))
        mot.move('rKickAngled', 0)
        phase = 'BallNotFound'
        return False
    
    print "Kick phase:", goal

    # else a goal is found, together with it's color
    (color , kickangles ) = goal
    (first,second) = kickangles 
    if first and second:
        kickangle = (2 * first + second) / 3.0 # kick slightly more to closest 
    elif first:
        kickangle = first
    
    if color == 'Blue':
        goalColor = 0
    else:
        goalColor = 1
        
    # Cases 1-3, if you see your own goal, kick to the other side
    if goalColor == teamColor:
        # Case 1, goal is left, kick to the right. 
        if kickangle >= 0.7:
            kickangle = -1
        # Case 2, goal is right, kick to the left.
        if kickangle <= -0.7:
            kickangle = 1
        else:
        # Case 3, goal is straight forward, HAK
            mot.move('walkTo',(0,0.04 + ball[1],0))
            mot.move('normalPose')
            time.sleep(0.5)
            leds.fadeRGB('LeftFaceLeds',0x00000000, 0) # led turns black
            mot.move('hakje', kickangle * -0.1)
            phase = 'BallNotFound'
            return True
            
    # Case 4, other player's goal is found.
    # Kick towards it. 
    if kickangle > 1.1:
        kickangle = 1.1
    if kickangle < -1.1:
        kickangle = -1.1
    
    # conversion to real kickangle
    convert = 1.1 # (60 degrees rad)
    
    # kick either left or right
    if kickangle <= 0:
        mot.move('walkTo', (0, -0.04 + ball[1], 0))
        #strafe(-0.1 + ball[1])
        mot.move('lKickAngled', kickangle/ -convert)
    elif kickangle > 0:
        mot.move('walkTo', (0, 0.04 + ball[1], 0))
        #strafe(0.1 + ball[1])
        mot.move('rKickAngled', kickangle/ convert)
    leds.fadeRGB('LeftFaceLeds', 0x00000000, 0)
    phase = 'BallNotFound'

'''
def strafe(y, x):
    now = time.time()
    while time.time() - now < 2:
        if -1 < 2.5 * y < 1:
            mot.SWTV((-0.07, y + x * ball[1], -0.1, 1))
        else:
            mot.SWTV((-0.07, math.copysign(1, y), -0.1, 1))
    mot.SWTV((-0.0001,0,0,0.8))
    time.sleep(2)
    mot.SWTV((0,0,0,0))
'''
    
# the plan is, since you start at the side..
def Unpenalized():
    global phase
    vis.goBall()
    # walk forward (2 meters)
    if not(mot.isWalking()):
        mot.move('postWalkTo', (2, 0, 0))
    phase = 'ReturnField'

# and while you're walking forward
def ReturnField():
    global phase
    if mot.isWalking():
        # search for a ball
        if v.scanCircle(0.2, vis):
            # extra check to see if it's not incidental
            ball = vis.findBall()
            if ball:
                phase = 'BallFound'
    else:
        phase = 'BallNotFound'

# Constant updating the state
class stateUpdate(threading.Thread):
    def __init__(self, name):
        threading.Thread.__init__(self)
        self.name = name

    def run(self):
        global state
        while(1):
            state = sc.getState()
        print 'stopped'
update = stateUpdate("stateUpdate")

##### OPERATING SYSTEM #####
# The soul of the robot

def awakeSoul():
    global state
    sc.start()
    update.start()
    mot.stiff()
    while(1):
        #print 'soul.py State: ', state
        states.get(state)()
   
# DICTIONARIES
# STATES
states =     {
    0: Initial,
    1: Ready,
    2: Set,
    3: Playing,
    4: Finished,
    10: Penalized
    }

# PHASES
phases =     {
    'BallFound': BallFound,
    'BallNotFound': BallNotFound,
    'Kick' : Kick,
    'BallFoundKeep': BallFoundKeep,
    'BallNotFoundKeep': BallNotFoundKeep,
    'BallApproaching': BallApproaching,
    'InGoalArea': InGoalArea,
    'Unpenalized': Unpenalized,
    'ReturnField': ReturnField
}

awakeSoul()

# DEBUG #
def testPhase(phase, interval):
    now = time.time()
    while time.time() - now < interval:
        phases.get(phase)()